// Copyright 2024 Hongbiao Zhu
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Original work based on sensor_scan_generation package by Hongbiao Zhu.

// Original work based on sensor_scan_generation package by Hongbiao Zhu.

#include <math.h>

#include "nav_msgs/msg/odometry.hpp"
#include "pcl/filters/voxel_grid.h"
#include "pcl/point_cloud.h"
#include "pcl/point_types.h"
#include "pcl_conversions/pcl_conversions.h"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/joy.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "std_msgs/msg/float32.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/transform_broadcaster.h"

double scanVoxelSize = 0.05;
double decayTime = 2.0;
double noDecayDis = 4.0;
double clearingDis = 8.0;
bool clearingCloud = false;
bool useSorting = true;
double quantileZ = 0.25;
bool considerDrop = false;
bool limitGroundLift = false;
double maxGroundLift = 0.15;
bool clearDyObs = false;
double minDyObsDis = 0.3;
double minDyObsAngle = 0;
double minDyObsRelZ = -0.5;
double absDyObsRelZThre = 0.2;
double minDyObsVFOV = -16.0;
double maxDyObsVFOV = 16.0;
int minDyObsPointNum = 1;
bool noDataObstacle = false;
int noDataBlockSkipNum = 0;
int minBlockPointNum = 10;
double vehicleHeight = 1.5;
int voxelPointUpdateThre = 100;
double voxelTimeUpdateThre = 2.0;
double minRelZ = -1.5;
double maxRelZ = 0.2;
double disRatioZ = 0.2;

// terrain voxel parameters
float terrainVoxelSize = 1.0;
int terrainVoxelShiftX = 0;
int terrainVoxelShiftY = 0;
const int terrainVoxelWidth = 21;
int terrainVoxelHalfWidth = (terrainVoxelWidth - 1) / 2;
constexpr int kTerrainVoxelNum = terrainVoxelWidth * terrainVoxelWidth;

// planar voxel parameters
float planarVoxelSize = 0.2;
const int planarVoxelWidth = 51;
int planarVoxelHalfWidth = (planarVoxelWidth - 1) / 2;
constexpr int kPlanarVoxelNum = planarVoxelWidth * planarVoxelWidth;

pcl::PointCloud<pcl::PointXYZI>::Ptr
    laserCloud(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr
    laserCloudCrop(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr
    laserCloudDwz(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr
    terrainCloud(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr
    terrainCloudElev(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr terrainVoxelCloud[kTerrainVoxelNum];

int terrainVoxelUpdateNum[kTerrainVoxelNum] = {0};
float terrainVoxelUpdateTime[kTerrainVoxelNum] = {0};
float planarVoxelElev[kPlanarVoxelNum] = {0};
int planarVoxelEdge[kPlanarVoxelNum] = {0};
int planarVoxelDyObs[kPlanarVoxelNum] = {0};
std::vector<float> planarPointElev[kPlanarVoxelNum];

double laserCloudTime = 0;
bool newlaserCloud = false;

double systemInitTime = 0;
bool systemInited = false;
int noDataInited = 0;

float vehicleRoll = 0, vehiclePitch = 0, vehicleYaw = 0;
float vehicleX = 0, vehicleY = 0, vehicleZ = 0;
float vehicleXRec = 0, vehicleYRec = 0;

float sinVehicleRoll = 0, cosVehicleRoll = 0;
float sinVehiclePitch = 0, cosVehiclePitch = 0;
float sinVehicleYaw = 0, cosVehicleYaw = 0;

pcl::VoxelGrid<pcl::PointXYZI> downSizeFilter;

// ========== 新增：3D空间障碍物检测相关变量 ==========

// 3D空间体素网格常量
const int spatialVoxelWidth = 21;
const int spatialVoxelHeight = 15;  // Z轴方向的体素数量
int spatialVoxelHalfWidth = (spatialVoxelWidth - 1) / 2;
int spatialVoxelHalfHeight = (spatialVoxelHeight - 1) / 2;
constexpr int kSpatialVoxelNum = spatialVoxelWidth * spatialVoxelWidth * spatialVoxelHeight;

// 3D空间体素数据结构
pcl::PointCloud<pcl::PointXYZI>::Ptr spatialVoxelCloud[kSpatialVoxelNum];
int spatialVoxelUpdateNum[kSpatialVoxelNum] = {0};
float spatialVoxelUpdateTime[kSpatialVoxelNum] = {0};
float spatialVoxelOccupancy[kSpatialVoxelNum] = {0};  // 体素占用概率

// 3D分析参数
float spatialVoxelSize = 0.5;    // 3D体素大小
int spatialVoxelShiftX = 0;      // X轴体素偏移
int spatialVoxelShiftY = 0;      // Y轴体素偏移 
int spatialVoxelShiftZ = 0;      // Z轴体素偏移
float droneMinHeight = -2.0;     // 无人机最低飞行高度(相对)
float droneMaxHeight = 10.0;     // 无人机最大飞行高度(相对)
float obstacleThreshold = 0.6;   // 障碍物占用概率阈值
float decayFactor = 0.95;        // 体素衰减因子
bool enableSpatialAnalysis = true; // 启用3D空间分析

// 3D障碍物点云输出
pcl::PointCloud<pcl::PointXYZI>::Ptr spatialObstacleCloud(new pcl::PointCloud<pcl::PointXYZI>());

// state estimation callback function
void odometryHandler(const nav_msgs::msg::Odometry::ConstSharedPtr odom) {
  double roll, pitch, yaw;
  geometry_msgs::msg::Quaternion geoQuat = odom->pose.pose.orientation;
  tf2::Matrix3x3(tf2::Quaternion(geoQuat.x, geoQuat.y, geoQuat.z, geoQuat.w))
      .getRPY(roll, pitch, yaw);

  vehicleRoll = roll;
  vehiclePitch = pitch;
  vehicleYaw = yaw;
  vehicleX = odom->pose.pose.position.x;
  vehicleY = odom->pose.pose.position.y;
  vehicleZ = odom->pose.pose.position.z;

  sinVehicleRoll = sin(vehicleRoll);
  cosVehicleRoll = cos(vehicleRoll);
  sinVehiclePitch = sin(vehiclePitch);
  cosVehiclePitch = cos(vehiclePitch);
  sinVehicleYaw = sin(vehicleYaw);
  cosVehicleYaw = cos(vehicleYaw);

  if (noDataInited == 0) {
    vehicleXRec = vehicleX;
    vehicleYRec = vehicleY;
    noDataInited = 1;
  }
  if (noDataInited == 1) {
    float dis = sqrt((vehicleX - vehicleXRec) * (vehicleX - vehicleXRec) +
                     (vehicleY - vehicleYRec) * (vehicleY - vehicleYRec));
    if (dis >= noDecayDis)
      noDataInited = 2;
  }
}

// registered laser scan callback function
void laserCloudHandler(
    const sensor_msgs::msg::PointCloud2::ConstSharedPtr laserCloud2) {
  laserCloudTime = rclcpp::Time(laserCloud2->header.stamp).seconds();
  if (!systemInited) {
    systemInitTime = laserCloudTime;
    systemInited = true;
  }

  laserCloud->clear();
  pcl::fromROSMsg(*laserCloud2, *laserCloud);

  pcl::PointXYZI point;
  laserCloudCrop->clear();
  int laserCloudSize = laserCloud->points.size();
  for (int i = 0; i < laserCloudSize; i++) {
    point = laserCloud->points[i];

    float pointX = point.x;
    float pointY = point.y;
    float pointZ = point.z;

    float dis = sqrt((pointX - vehicleX) * (pointX - vehicleX) +
                     (pointY - vehicleY) * (pointY - vehicleY));
    if (pointZ - vehicleZ > minRelZ - disRatioZ * dis &&
        pointZ - vehicleZ < maxRelZ + disRatioZ * dis &&
        dis < terrainVoxelSize * (terrainVoxelHalfWidth + 1)) {
      point.x = pointX;
      point.y = pointY;
      point.z = pointZ;
      point.intensity = laserCloudTime - systemInitTime;
      laserCloudCrop->push_back(point);
    }
  }

  newlaserCloud = true;
}

// joystick callback function
void joystickHandler(const sensor_msgs::msg::Joy::ConstSharedPtr joy) {
  if (joy->buttons[5] > 0.5) {
    noDataInited = 0;
    clearingCloud = true;
  }
}

// cloud clearing callback function
void clearingHandler(const std_msgs::msg::Float32::ConstSharedPtr dis) {
  noDataInited = 0;
  clearingDis = dis->data;
  clearingCloud = true;
}

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto nh = rclcpp::Node::make_shared("terrainAnalysis");

  nh->declare_parameter<double>("scanVoxelSize", scanVoxelSize);
  nh->declare_parameter<double>("decayTime", decayTime);
  nh->declare_parameter<double>("noDecayDis", noDecayDis);
  nh->declare_parameter<double>("clearingDis", clearingDis);
  nh->declare_parameter<bool>("useSorting", useSorting);
  nh->declare_parameter<double>("quantileZ", quantileZ);
  nh->declare_parameter<bool>("considerDrop", considerDrop);
  nh->declare_parameter<bool>("limitGroundLift", limitGroundLift);
  nh->declare_parameter<double>("maxGroundLift", maxGroundLift);
  nh->declare_parameter<bool>("clearDyObs", clearDyObs);
  nh->declare_parameter<double>("minDyObsDis", minDyObsDis);
  nh->declare_parameter<double>("minDyObsAngle", minDyObsAngle);
  nh->declare_parameter<double>("minDyObsRelZ", minDyObsRelZ);
  nh->declare_parameter<double>("absDyObsRelZThre", absDyObsRelZThre);
  nh->declare_parameter<double>("minDyObsVFOV", minDyObsVFOV);
  nh->declare_parameter<double>("maxDyObsVFOV", maxDyObsVFOV);
  nh->declare_parameter<int>("minDyObsPointNum", minDyObsPointNum);
  nh->declare_parameter<bool>("noDataObstacle", noDataObstacle);
  nh->declare_parameter<int>("noDataBlockSkipNum", noDataBlockSkipNum);
  nh->declare_parameter<int>("minBlockPointNum", minBlockPointNum);
  nh->declare_parameter<double>("vehicleHeight", vehicleHeight);
  nh->declare_parameter<int>("voxelPointUpdateThre", voxelPointUpdateThre);
  nh->declare_parameter<double>("voxelTimeUpdateThre", voxelTimeUpdateThre);
  nh->declare_parameter<double>("minRelZ", minRelZ);
  nh->declare_parameter<double>("maxRelZ", maxRelZ);
  nh->declare_parameter<double>("disRatioZ", disRatioZ);
  
  // 3D空间分析参数声明
  nh->declare_parameter<float>("spatialVoxelSize", spatialVoxelSize);
  nh->declare_parameter<float>("droneMinHeight", droneMinHeight);
  nh->declare_parameter<float>("droneMaxHeight", droneMaxHeight);
  nh->declare_parameter<float>("obstacleThreshold", obstacleThreshold);
  nh->declare_parameter<float>("decayFactor", decayFactor);
  nh->declare_parameter<bool>("enableSpatialAnalysis", enableSpatialAnalysis);

  nh->get_parameter("scanVoxelSize", scanVoxelSize);
  nh->get_parameter("decayTime", decayTime);
  nh->get_parameter("noDecayDis", noDecayDis);
  nh->get_parameter("clearingDis", clearingDis);
  nh->get_parameter("useSorting", useSorting);
  nh->get_parameter("quantileZ", quantileZ);
  nh->get_parameter("considerDrop", considerDrop);
  nh->get_parameter("limitGroundLift", limitGroundLift);
  nh->get_parameter("maxGroundLift", maxGroundLift);
  nh->get_parameter("clearDyObs", clearDyObs);
  nh->get_parameter("minDyObsDis", minDyObsDis);
  nh->get_parameter("minDyObsAngle", minDyObsAngle);
  nh->get_parameter("minDyObsRelZ", minDyObsRelZ);
  nh->get_parameter("absDyObsRelZThre", absDyObsRelZThre);
  nh->get_parameter("minDyObsVFOV", minDyObsVFOV);
  nh->get_parameter("maxDyObsVFOV", maxDyObsVFOV);
  nh->get_parameter("minDyObsPointNum", minDyObsPointNum);
  nh->get_parameter("noDataObstacle", noDataObstacle);
  nh->get_parameter("noDataBlockSkipNum", noDataBlockSkipNum);
  nh->get_parameter("minBlockPointNum", minBlockPointNum);
  nh->get_parameter("vehicleHeight", vehicleHeight);
  nh->get_parameter("voxelPointUpdateThre", voxelPointUpdateThre);
  nh->get_parameter("voxelTimeUpdateThre", voxelTimeUpdateThre);
  nh->get_parameter("minRelZ", minRelZ);
  nh->get_parameter("maxRelZ", maxRelZ);
  nh->get_parameter("disRatioZ", disRatioZ);
  
  // 3D空间分析参数获取
  nh->get_parameter("spatialVoxelSize", spatialVoxelSize);
  nh->get_parameter("droneMinHeight", droneMinHeight);
  nh->get_parameter("droneMaxHeight", droneMaxHeight);
  nh->get_parameter("obstacleThreshold", obstacleThreshold);
  nh->get_parameter("decayFactor", decayFactor);
  nh->get_parameter("enableSpatialAnalysis", enableSpatialAnalysis);

  auto subOdometry = nh->create_subscription<nav_msgs::msg::Odometry>(
      "lidar_odometry", 5, odometryHandler);

  auto subLaserCloud = nh->create_subscription<sensor_msgs::msg::PointCloud2>(
      "registered_scan", 5, laserCloudHandler);

  auto subJoystick =
      nh->create_subscription<sensor_msgs::msg::Joy>("joy", 5, joystickHandler);

  auto subClearing = nh->create_subscription<std_msgs::msg::Float32>(
      "map_clearing", 5, clearingHandler);

  auto pubLaserCloud =
      nh->create_publisher<sensor_msgs::msg::PointCloud2>("terrain_map", 2);
  
  // 3D障碍物点云发布者
  auto pubSpatialObstacle =
      nh->create_publisher<sensor_msgs::msg::PointCloud2>("spatial_obstacles", 2);

  for (int i = 0; i < kTerrainVoxelNum; i++) {
    terrainVoxelCloud[i].reset(new pcl::PointCloud<pcl::PointXYZI>());
  }
  
  // 初始化3D空间体素
  for (int i = 0; i < kSpatialVoxelNum; i++) {
    spatialVoxelCloud[i].reset(new pcl::PointCloud<pcl::PointXYZI>());
  }

  downSizeFilter.setLeafSize(scanVoxelSize, scanVoxelSize, scanVoxelSize);

  rclcpp::Rate rate(100);
  bool status = rclcpp::ok();
  while (status) {
    rclcpp::spin_some(nh);
    if (newlaserCloud) {
      newlaserCloud = false;

      // terrain voxel roll over
      float terrainVoxelCenX = terrainVoxelSize * terrainVoxelShiftX;
      float terrainVoxelCenY = terrainVoxelSize * terrainVoxelShiftY;

      while (vehicleX - terrainVoxelCenX < -terrainVoxelSize) {
        for (int indY = 0; indY < terrainVoxelWidth; indY++) {
          pcl::PointCloud<pcl::PointXYZI>::Ptr terrainVoxelCloudPtr =
              terrainVoxelCloud[terrainVoxelWidth * (terrainVoxelWidth - 1) +
                                indY];
          for (int indX = terrainVoxelWidth - 1; indX >= 1; indX--) {
            terrainVoxelCloud[terrainVoxelWidth * indX + indY] =
                terrainVoxelCloud[terrainVoxelWidth * (indX - 1) + indY];
          }
          terrainVoxelCloud[indY] = terrainVoxelCloudPtr;
          terrainVoxelCloud[indY]->clear();
        }
        terrainVoxelShiftX--;
        terrainVoxelCenX = terrainVoxelSize * terrainVoxelShiftX;
      }

      while (vehicleX - terrainVoxelCenX > terrainVoxelSize) {
        for (int indY = 0; indY < terrainVoxelWidth; indY++) {
          pcl::PointCloud<pcl::PointXYZI>::Ptr terrainVoxelCloudPtr =
              terrainVoxelCloud[indY];
          for (int indX = 0; indX < terrainVoxelWidth - 1; indX++) {
            terrainVoxelCloud[terrainVoxelWidth * indX + indY] =
                terrainVoxelCloud[terrainVoxelWidth * (indX + 1) + indY];
          }
          terrainVoxelCloud[terrainVoxelWidth * (terrainVoxelWidth - 1) +
                            indY] = terrainVoxelCloudPtr;
          terrainVoxelCloud[terrainVoxelWidth * (terrainVoxelWidth - 1) + indY]
              ->clear();
        }
        terrainVoxelShiftX++;
        terrainVoxelCenX = terrainVoxelSize * terrainVoxelShiftX;
      }

      while (vehicleY - terrainVoxelCenY < -terrainVoxelSize) {
        for (int indX = 0; indX < terrainVoxelWidth; indX++) {
          pcl::PointCloud<pcl::PointXYZI>::Ptr terrainVoxelCloudPtr =
              terrainVoxelCloud[terrainVoxelWidth * indX +
                                (terrainVoxelWidth - 1)];
          for (int indY = terrainVoxelWidth - 1; indY >= 1; indY--) {
            terrainVoxelCloud[terrainVoxelWidth * indX + indY] =
                terrainVoxelCloud[terrainVoxelWidth * indX + (indY - 1)];
          }
          terrainVoxelCloud[terrainVoxelWidth * indX] = terrainVoxelCloudPtr;
          terrainVoxelCloud[terrainVoxelWidth * indX]->clear();
        }
        terrainVoxelShiftY--;
        terrainVoxelCenY = terrainVoxelSize * terrainVoxelShiftY;
      }

      while (vehicleY - terrainVoxelCenY > terrainVoxelSize) {
        for (int indX = 0; indX < terrainVoxelWidth; indX++) {
          pcl::PointCloud<pcl::PointXYZI>::Ptr terrainVoxelCloudPtr =
              terrainVoxelCloud[terrainVoxelWidth * indX];
          for (int indY = 0; indY < terrainVoxelWidth - 1; indY++) {
            terrainVoxelCloud[terrainVoxelWidth * indX + indY] =
                terrainVoxelCloud[terrainVoxelWidth * indX + (indY + 1)];
          }
          terrainVoxelCloud[terrainVoxelWidth * indX +
                            (terrainVoxelWidth - 1)] = terrainVoxelCloudPtr;
          terrainVoxelCloud[terrainVoxelWidth * indX + (terrainVoxelWidth - 1)]
              ->clear();
        }
        terrainVoxelShiftY++;
        terrainVoxelCenY = terrainVoxelSize * terrainVoxelShiftY;
      }

      // ========== 3D空间体素滚动更新 ==========
      if (enableSpatialAnalysis) {
        float spatialVoxelCenX = spatialVoxelSize * spatialVoxelShiftX;
        float spatialVoxelCenY = spatialVoxelSize * spatialVoxelShiftY;
        float spatialVoxelCenZ = spatialVoxelSize * spatialVoxelShiftZ;

        // X轴滚动
        while (vehicleX - spatialVoxelCenX < -spatialVoxelSize) {
          for (int indY = 0; indY < spatialVoxelWidth; indY++) {
            for (int indZ = 0; indZ < spatialVoxelHeight; indZ++) {
              pcl::PointCloud<pcl::PointXYZI>::Ptr spatialVoxelCloudPtr =
                  spatialVoxelCloud[spatialVoxelWidth * spatialVoxelWidth * indZ +
                                   spatialVoxelWidth * (spatialVoxelWidth - 1) + indY];
              for (int indX = spatialVoxelWidth - 1; indX >= 1; indX--) {
                int currentIdx = spatialVoxelWidth * spatialVoxelWidth * indZ + 
                               spatialVoxelWidth * indX + indY;
                int nextIdx = spatialVoxelWidth * spatialVoxelWidth * indZ + 
                            spatialVoxelWidth * (indX - 1) + indY;
                spatialVoxelCloud[currentIdx] = spatialVoxelCloud[nextIdx];
                spatialVoxelOccupancy[currentIdx] = spatialVoxelOccupancy[nextIdx];
              }
              int resetIdx = spatialVoxelWidth * spatialVoxelWidth * indZ + indY;
              spatialVoxelCloud[resetIdx] = spatialVoxelCloudPtr;
              spatialVoxelCloud[resetIdx]->clear();
              spatialVoxelOccupancy[resetIdx] = 0.0;
            }
          }
          spatialVoxelShiftX--;
          spatialVoxelCenX = spatialVoxelSize * spatialVoxelShiftX;
        }

        while (vehicleX - spatialVoxelCenX > spatialVoxelSize) {
          for (int indY = 0; indY < spatialVoxelWidth; indY++) {
            for (int indZ = 0; indZ < spatialVoxelHeight; indZ++) {
              pcl::PointCloud<pcl::PointXYZI>::Ptr spatialVoxelCloudPtr =
                  spatialVoxelCloud[spatialVoxelWidth * spatialVoxelWidth * indZ + indY];
              for (int indX = 0; indX < spatialVoxelWidth - 1; indX++) {
                int currentIdx = spatialVoxelWidth * spatialVoxelWidth * indZ + 
                               spatialVoxelWidth * indX + indY;
                int nextIdx = spatialVoxelWidth * spatialVoxelWidth * indZ + 
                            spatialVoxelWidth * (indX + 1) + indY;
                spatialVoxelCloud[currentIdx] = spatialVoxelCloud[nextIdx];
                spatialVoxelOccupancy[currentIdx] = spatialVoxelOccupancy[nextIdx];
              }
              int resetIdx = spatialVoxelWidth * spatialVoxelWidth * indZ + 
                           spatialVoxelWidth * (spatialVoxelWidth - 1) + indY;
              spatialVoxelCloud[resetIdx] = spatialVoxelCloudPtr;
              spatialVoxelCloud[resetIdx]->clear();
              spatialVoxelOccupancy[resetIdx] = 0.0;
            }
          }
          spatialVoxelShiftX++;
          spatialVoxelCenX = spatialVoxelSize * spatialVoxelShiftX;
        }

        // Y轴滚动
        while (vehicleY - spatialVoxelCenY < -spatialVoxelSize) {
          for (int indX = 0; indX < spatialVoxelWidth; indX++) {
            for (int indZ = 0; indZ < spatialVoxelHeight; indZ++) {
              pcl::PointCloud<pcl::PointXYZI>::Ptr spatialVoxelCloudPtr =
                  spatialVoxelCloud[spatialVoxelWidth * spatialVoxelWidth * indZ +
                                   spatialVoxelWidth * indX + (spatialVoxelWidth - 1)];
              for (int indY = spatialVoxelWidth - 1; indY >= 1; indY--) {
                int currentIdx = spatialVoxelWidth * spatialVoxelWidth * indZ + 
                               spatialVoxelWidth * indX + indY;
                int nextIdx = spatialVoxelWidth * spatialVoxelWidth * indZ + 
                            spatialVoxelWidth * indX + (indY - 1);
                spatialVoxelCloud[currentIdx] = spatialVoxelCloud[nextIdx];
                spatialVoxelOccupancy[currentIdx] = spatialVoxelOccupancy[nextIdx];
              }
              int resetIdx = spatialVoxelWidth * spatialVoxelWidth * indZ + 
                           spatialVoxelWidth * indX;
              spatialVoxelCloud[resetIdx] = spatialVoxelCloudPtr;
              spatialVoxelCloud[resetIdx]->clear();
              spatialVoxelOccupancy[resetIdx] = 0.0;
            }
          }
          spatialVoxelShiftY--;
          spatialVoxelCenY = spatialVoxelSize * spatialVoxelShiftY;
        }

        while (vehicleY - spatialVoxelCenY > spatialVoxelSize) {
          for (int indX = 0; indX < spatialVoxelWidth; indX++) {
            for (int indZ = 0; indZ < spatialVoxelHeight; indZ++) {
              pcl::PointCloud<pcl::PointXYZI>::Ptr spatialVoxelCloudPtr =
                  spatialVoxelCloud[spatialVoxelWidth * spatialVoxelWidth * indZ +
                                   spatialVoxelWidth * indX];
              for (int indY = 0; indY < spatialVoxelWidth - 1; indY++) {
                int currentIdx = spatialVoxelWidth * spatialVoxelWidth * indZ + 
                               spatialVoxelWidth * indX + indY;
                int nextIdx = spatialVoxelWidth * spatialVoxelWidth * indZ + 
                            spatialVoxelWidth * indX + (indY + 1);
                spatialVoxelCloud[currentIdx] = spatialVoxelCloud[nextIdx];
                spatialVoxelOccupancy[currentIdx] = spatialVoxelOccupancy[nextIdx];
              }
              int resetIdx = spatialVoxelWidth * spatialVoxelWidth * indZ + 
                           spatialVoxelWidth * indX + (spatialVoxelWidth - 1);
              spatialVoxelCloud[resetIdx] = spatialVoxelCloudPtr;
              spatialVoxelCloud[resetIdx]->clear();
              spatialVoxelOccupancy[resetIdx] = 0.0;
            }
          }
          spatialVoxelShiftY++;
          spatialVoxelCenY = spatialVoxelSize * spatialVoxelShiftY;
        }

        // Z轴滚动（新增，适应无人机高度变化）
        while (vehicleZ - spatialVoxelCenZ < droneMinHeight) {
          for (int indX = 0; indX < spatialVoxelWidth; indX++) {
            for (int indY = 0; indY < spatialVoxelWidth; indY++) {
              pcl::PointCloud<pcl::PointXYZI>::Ptr spatialVoxelCloudPtr =
                  spatialVoxelCloud[spatialVoxelWidth * spatialVoxelWidth * (spatialVoxelHeight - 1) +
                                   spatialVoxelWidth * indX + indY];
              for (int indZ = spatialVoxelHeight - 1; indZ >= 1; indZ--) {
                int currentIdx = spatialVoxelWidth * spatialVoxelWidth * indZ + 
                               spatialVoxelWidth * indX + indY;
                int nextIdx = spatialVoxelWidth * spatialVoxelWidth * (indZ - 1) + 
                            spatialVoxelWidth * indX + indY;
                spatialVoxelCloud[currentIdx] = spatialVoxelCloud[nextIdx];
                spatialVoxelOccupancy[currentIdx] = spatialVoxelOccupancy[nextIdx];
              }
              int resetIdx = spatialVoxelWidth * indX + indY;
              spatialVoxelCloud[resetIdx] = spatialVoxelCloudPtr;
              spatialVoxelCloud[resetIdx]->clear();
              spatialVoxelOccupancy[resetIdx] = 0.0;
            }
          }
          spatialVoxelShiftZ--;
          spatialVoxelCenZ = spatialVoxelSize * spatialVoxelShiftZ;
        }

        while (vehicleZ - spatialVoxelCenZ > droneMaxHeight) {
          for (int indX = 0; indX < spatialVoxelWidth; indX++) {
            for (int indY = 0; indY < spatialVoxelWidth; indY++) {
              pcl::PointCloud<pcl::PointXYZI>::Ptr spatialVoxelCloudPtr =
                  spatialVoxelCloud[spatialVoxelWidth * indX + indY];
              for (int indZ = 0; indZ < spatialVoxelHeight - 1; indZ++) {
                int currentIdx = spatialVoxelWidth * spatialVoxelWidth * indZ + 
                               spatialVoxelWidth * indX + indY;
                int nextIdx = spatialVoxelWidth * spatialVoxelWidth * (indZ + 1) + 
                            spatialVoxelWidth * indX + indY;
                spatialVoxelCloud[currentIdx] = spatialVoxelCloud[nextIdx];
                spatialVoxelOccupancy[currentIdx] = spatialVoxelOccupancy[nextIdx];
              }
              int resetIdx = spatialVoxelWidth * spatialVoxelWidth * (spatialVoxelHeight - 1) +
                           spatialVoxelWidth * indX + indY;
              spatialVoxelCloud[resetIdx] = spatialVoxelCloudPtr;
              spatialVoxelCloud[resetIdx]->clear();
              spatialVoxelOccupancy[resetIdx] = 0.0;
            }
          }
          spatialVoxelShiftZ++;
          spatialVoxelCenZ = spatialVoxelSize * spatialVoxelShiftZ;
        }
      }

      // stack registered laser scans
      pcl::PointXYZI point;
      int laserCloudCropSize = laserCloudCrop->points.size();
      for (int i = 0; i < laserCloudCropSize; i++) {
        point = laserCloudCrop->points[i];

        int indX =
            static_cast<int>((point.x - vehicleX + terrainVoxelSize / 2) /
                             terrainVoxelSize) +
            terrainVoxelHalfWidth;
        int indY =
            static_cast<int>((point.y - vehicleY + terrainVoxelSize / 2) /
                             terrainVoxelSize) +
            terrainVoxelHalfWidth;

        if (point.x - vehicleX + terrainVoxelSize / 2 < 0)
          indX--;
        if (point.y - vehicleY + terrainVoxelSize / 2 < 0)
          indY--;

        if (indX >= 0 && indX < terrainVoxelWidth && indY >= 0 &&
            indY < terrainVoxelWidth) {
          terrainVoxelCloud[terrainVoxelWidth * indX + indY]->push_back(point);
          terrainVoxelUpdateNum[terrainVoxelWidth * indX + indY]++;
        }

        // ========== 3D空间体素点云堆叠 ==========
        if (enableSpatialAnalysis) {
          int spatialIndX = static_cast<int>((point.x - vehicleX + spatialVoxelSize / 2) /
                                           spatialVoxelSize) + spatialVoxelHalfWidth;
          int spatialIndY = static_cast<int>((point.y - vehicleY + spatialVoxelSize / 2) /
                                           spatialVoxelSize) + spatialVoxelHalfWidth;
          int spatialIndZ = static_cast<int>((point.z - vehicleZ + spatialVoxelSize / 2) /
                                           spatialVoxelSize) + spatialVoxelHalfHeight;

          if (point.x - vehicleX + spatialVoxelSize / 2 < 0) spatialIndX--;
          if (point.y - vehicleY + spatialVoxelSize / 2 < 0) spatialIndY--;
          if (point.z - vehicleZ + spatialVoxelSize / 2 < 0) spatialIndZ--;

          // 检查是否在无人机飞行范围内且在3D体素网格范围内
          if (spatialIndX >= 0 && spatialIndX < spatialVoxelWidth &&
              spatialIndY >= 0 && spatialIndY < spatialVoxelWidth &&
              spatialIndZ >= 0 && spatialIndZ < spatialVoxelHeight &&
              point.z - vehicleZ >= droneMinHeight &&
              point.z - vehicleZ <= droneMaxHeight) {
            
            int spatialVoxelIdx = spatialVoxelWidth * spatialVoxelWidth * spatialIndZ +
                                spatialVoxelWidth * spatialIndX + spatialIndY;
            
            spatialVoxelCloud[spatialVoxelIdx]->push_back(point);
            spatialVoxelUpdateNum[spatialVoxelIdx]++;
          }
        }
      }

      for (int ind = 0; ind < kTerrainVoxelNum; ind++) {
        if (terrainVoxelUpdateNum[ind] >= voxelPointUpdateThre ||
            laserCloudTime - systemInitTime - terrainVoxelUpdateTime[ind] >=
                voxelTimeUpdateThre ||
            clearingCloud) {
          pcl::PointCloud<pcl::PointXYZI>::Ptr terrainVoxelCloudPtr =
              terrainVoxelCloud[ind];

          laserCloudDwz->clear();
          downSizeFilter.setInputCloud(terrainVoxelCloudPtr);
          downSizeFilter.filter(*laserCloudDwz);

          terrainVoxelCloudPtr->clear();
          int laserCloudDwzSize = laserCloudDwz->points.size();
          for (int i = 0; i < laserCloudDwzSize; i++) {
            point = laserCloudDwz->points[i];
            float dis = sqrt((point.x - vehicleX) * (point.x - vehicleX) +
                             (point.y - vehicleY) * (point.y - vehicleY));
            if (point.z - vehicleZ > minRelZ - disRatioZ * dis &&
                point.z - vehicleZ < maxRelZ + disRatioZ * dis &&
                (laserCloudTime - systemInitTime - point.intensity <
                     decayTime ||
                 dis < noDecayDis) &&
                !(dis < clearingDis && clearingCloud)) {
              terrainVoxelCloudPtr->push_back(point);
            }
          }

          terrainVoxelUpdateNum[ind] = 0;
          terrainVoxelUpdateTime[ind] = laserCloudTime - systemInitTime;
        }
      }

      // ========== 3D空间体素更新和占用计算 ==========
      if (enableSpatialAnalysis) {
        for (int ind = 0; ind < kSpatialVoxelNum; ind++) {
          if (spatialVoxelUpdateNum[ind] >= voxelPointUpdateThre ||
              laserCloudTime - systemInitTime - spatialVoxelUpdateTime[ind] >= voxelTimeUpdateThre ||
              clearingCloud) {
            
            pcl::PointCloud<pcl::PointXYZI>::Ptr spatialVoxelCloudPtr = spatialVoxelCloud[ind];
            
            // 体素衰减
            spatialVoxelOccupancy[ind] *= decayFactor;
            
            // 计算占用概率
            int pointCount = spatialVoxelCloudPtr->points.size();
            if (pointCount > 0) {
              // 根据点云密度更新占用概率
              float newOccupancy = std::min(1.0f, pointCount / 20.0f);  // 20个点视为完全占用
              spatialVoxelOccupancy[ind] = std::max(spatialVoxelOccupancy[ind], newOccupancy);
            }
            
            // 下采样
            laserCloudDwz->clear();
            downSizeFilter.setInputCloud(spatialVoxelCloudPtr);
            downSizeFilter.filter(*laserCloudDwz);
            
            // 保留经过时间和距离过滤的点云
            spatialVoxelCloudPtr->clear();
            int laserCloudDwzSize = laserCloudDwz->points.size();
            for (int i = 0; i < laserCloudDwzSize; i++) {
              pcl::PointXYZI spoint = laserCloudDwz->points[i];
              float dis = sqrt((spoint.x - vehicleX) * (spoint.x - vehicleX) +
                             (spoint.y - vehicleY) * (spoint.y - vehicleY) +
                             (spoint.z - vehicleZ) * (spoint.z - vehicleZ));
              
              if (spoint.z - vehicleZ >= droneMinHeight &&
                  spoint.z - vehicleZ <= droneMaxHeight &&
                  (laserCloudTime - systemInitTime - spoint.intensity < decayTime ||
                   dis < noDecayDis) &&
                  !(dis < clearingDis && clearingCloud)) {
                spatialVoxelCloudPtr->push_back(spoint);
              }
            }
            
            spatialVoxelUpdateNum[ind] = 0;
            spatialVoxelUpdateTime[ind] = laserCloudTime - systemInitTime;
          }
        }
      }

      terrainCloud->clear();
      for (int indX = terrainVoxelHalfWidth - 5;
           indX <= terrainVoxelHalfWidth + 5; indX++) {
        for (int indY = terrainVoxelHalfWidth - 5;
             indY <= terrainVoxelHalfWidth + 5; indY++) {
          *terrainCloud += *terrainVoxelCloud[terrainVoxelWidth * indX + indY];
        }
      }

      // estimate ground and compute elevation for each point
      for (int i = 0; i < kPlanarVoxelNum; i++) {
        planarVoxelElev[i] = 0;
        planarVoxelEdge[i] = 0;
        planarVoxelDyObs[i] = 0;
        planarPointElev[i].clear();
      }

      int terrainCloudSize = terrainCloud->points.size();
      for (int i = 0; i < terrainCloudSize; i++) {
        point = terrainCloud->points[i];

        int indX = static_cast<int>((point.x - vehicleX + planarVoxelSize / 2) /
                                    planarVoxelSize) +
                   planarVoxelHalfWidth;
        int indY = static_cast<int>((point.y - vehicleY + planarVoxelSize / 2) /
                                    planarVoxelSize) +
                   planarVoxelHalfWidth;

        if (point.x - vehicleX + planarVoxelSize / 2 < 0)
          indX--;
        if (point.y - vehicleY + planarVoxelSize / 2 < 0)
          indY--;

        if (point.z - vehicleZ > minRelZ && point.z - vehicleZ < maxRelZ) {
          for (int dX = -1; dX <= 1; dX++) {
            for (int dY = -1; dY <= 1; dY++) {
              if (indX + dX >= 0 && indX + dX < planarVoxelWidth &&
                  indY + dY >= 0 && indY + dY < planarVoxelWidth) {
                planarPointElev[planarVoxelWidth * (indX + dX) + indY + dY]
                    .push_back(point.z);
              }
            }
          }
        }

        if (clearDyObs) {
          if (indX >= 0 && indX < planarVoxelWidth && indY >= 0 &&
              indY < planarVoxelWidth) {
            float pointX1 = point.x - vehicleX;
            float pointY1 = point.y - vehicleY;
            float pointZ1 = point.z - vehicleZ;

            float dis1 = sqrt(pointX1 * pointX1 + pointY1 * pointY1);
            if (dis1 > minDyObsDis) {
              float angle1 = atan2(pointZ1 - minDyObsRelZ, dis1) * 180.0 / M_PI;
              if (angle1 > minDyObsAngle) {
                float pointX2 =
                    pointX1 * cosVehicleYaw + pointY1 * sinVehicleYaw;
                float pointY2 =
                    -pointX1 * sinVehicleYaw + pointY1 * cosVehicleYaw;
                float pointZ2 = pointZ1;

                float pointX3 =
                    pointX2 * cosVehiclePitch - pointZ2 * sinVehiclePitch;
                float pointY3 = pointY2;
                float pointZ3 =
                    pointX2 * sinVehiclePitch + pointZ2 * cosVehiclePitch;

                float pointX4 = pointX3;
                float pointY4 =
                    pointY3 * cosVehicleRoll + pointZ3 * sinVehicleRoll;
                float pointZ4 =
                    -pointY3 * sinVehicleRoll + pointZ3 * cosVehicleRoll;

                float dis4 = sqrt(pointX4 * pointX4 + pointY4 * pointY4);
                float angle4 = atan2(pointZ4, dis4) * 180.0 / M_PI;
                if ((angle4 > minDyObsVFOV && angle4 < maxDyObsVFOV) ||
                    fabs(pointZ4) < absDyObsRelZThre) {
                  planarVoxelDyObs[planarVoxelWidth * indX + indY]++;
                }
              }
            } else {
              planarVoxelDyObs[planarVoxelWidth * indX + indY] +=
                  minDyObsPointNum;
            }
          }
        }
      }

      if (clearDyObs) {
        for (int i = 0; i < laserCloudCropSize; i++) {
          point = laserCloudCrop->points[i];

          int indX =
              static_cast<int>((point.x - vehicleX + planarVoxelSize / 2) /
                               planarVoxelSize) +
              planarVoxelHalfWidth;
          int indY =
              static_cast<int>((point.y - vehicleY + planarVoxelSize / 2) /
                               planarVoxelSize) +
              planarVoxelHalfWidth;

          if (point.x - vehicleX + planarVoxelSize / 2 < 0)
            indX--;
          if (point.y - vehicleY + planarVoxelSize / 2 < 0)
            indY--;

          if (indX >= 0 && indX < planarVoxelWidth && indY >= 0 &&
              indY < planarVoxelWidth) {
            float pointX1 = point.x - vehicleX;
            float pointY1 = point.y - vehicleY;
            float pointZ1 = point.z - vehicleZ;

            float dis1 = sqrt(pointX1 * pointX1 + pointY1 * pointY1);
            float angle1 = atan2(pointZ1 - minDyObsRelZ, dis1) * 180.0 / M_PI;
            if (angle1 > minDyObsAngle) {
              planarVoxelDyObs[planarVoxelWidth * indX + indY] = 0;
            }
          }
        }
      }

      if (useSorting) {
        for (int i = 0; i < kPlanarVoxelNum; i++) {
          int planarPointElevSize = planarPointElev[i].size();
          if (planarPointElevSize > 0) {
            sort(planarPointElev[i].begin(), planarPointElev[i].end());

            int quantileID = static_cast<int>(quantileZ * planarPointElevSize);
            if (quantileID < 0)
              quantileID = 0;
            else if (quantileID >= planarPointElevSize)
              quantileID = planarPointElevSize - 1;

            if (planarPointElev[i][quantileID] >
                    planarPointElev[i][0] + maxGroundLift &&
                limitGroundLift) {
              planarVoxelElev[i] = planarPointElev[i][0] + maxGroundLift;
            } else {
              planarVoxelElev[i] = planarPointElev[i][quantileID];
            }
          }
        }
      } else {
        for (int i = 0; i < kPlanarVoxelNum; i++) {
          int planarPointElevSize = planarPointElev[i].size();
          if (planarPointElevSize > 0) {
            float minZ = 1000.0;
            int minID = -1;
            for (int j = 0; j < planarPointElevSize; j++) {
              if (planarPointElev[i][j] < minZ) {
                minZ = planarPointElev[i][j];
                minID = j;
              }
            }

            if (minID != -1) {
              planarVoxelElev[i] = planarPointElev[i][minID];
            }
          }
        }
      }

      terrainCloudElev->clear();
      int terrainCloudElevSize = 0;
      for (int i = 0; i < terrainCloudSize; i++) {
        point = terrainCloud->points[i];
        if (point.z - vehicleZ > minRelZ && point.z - vehicleZ < maxRelZ) {
          int indX =
              static_cast<int>((point.x - vehicleX + planarVoxelSize / 2) /
                               planarVoxelSize) +
              planarVoxelHalfWidth;
          int indY =
              static_cast<int>((point.y - vehicleY + planarVoxelSize / 2) /
                               planarVoxelSize) +
              planarVoxelHalfWidth;

          if (point.x - vehicleX + planarVoxelSize / 2 < 0)
            indX--;
          if (point.y - vehicleY + planarVoxelSize / 2 < 0)
            indY--;

          if (indX >= 0 && indX < planarVoxelWidth && indY >= 0 &&
              indY < planarVoxelWidth) {
            if (planarVoxelDyObs[planarVoxelWidth * indX + indY] <
                    minDyObsPointNum ||
                !clearDyObs) {
              float disZ =
                  point.z - planarVoxelElev[planarVoxelWidth * indX + indY];
              if (considerDrop)
                disZ = fabs(disZ);
              int planarPointElevSize =
                  planarPointElev[planarVoxelWidth * indX + indY].size();
              if (disZ >= 0 && disZ < vehicleHeight &&
                  planarPointElevSize >= minBlockPointNum) {
                terrainCloudElev->push_back(point);
                terrainCloudElev->points[terrainCloudElevSize].intensity = disZ;

                terrainCloudElevSize++;
              }
            }
          }
        }
      }

      if (noDataObstacle && noDataInited == 2) {
        for (int i = 0; i < kPlanarVoxelNum; i++) {
          int planarPointElevSize = planarPointElev[i].size();
          if (planarPointElevSize < minBlockPointNum) {
            planarVoxelEdge[i] = 1;
          }
        }

        for (int noDataBlockSkipCount = 0;
             noDataBlockSkipCount < noDataBlockSkipNum;
             noDataBlockSkipCount++) {
          for (int i = 0; i < kPlanarVoxelNum; i++) {
            if (planarVoxelEdge[i] >= 1) {
              int indX = static_cast<int>(i / planarVoxelWidth);
              int indY = i % planarVoxelWidth;
              bool edgeVoxel = false;
              for (int dX = -1; dX <= 1; dX++) {
                for (int dY = -1; dY <= 1; dY++) {
                  if (indX + dX >= 0 && indX + dX < planarVoxelWidth &&
                      indY + dY >= 0 && indY + dY < planarVoxelWidth) {
                    if (planarVoxelEdge[planarVoxelWidth * (indX + dX) + indY +
                                        dY] < planarVoxelEdge[i]) {
                      edgeVoxel = true;
                    }
                  }
                }
              }

              if (!edgeVoxel)
                planarVoxelEdge[i]++;
            }
          }
        }

        for (int i = 0; i < kPlanarVoxelNum; i++) {
          if (planarVoxelEdge[i] > noDataBlockSkipNum) {
            int indX = static_cast<int>(i / planarVoxelWidth);
            int indY = i % planarVoxelWidth;

            point.x =
                planarVoxelSize * (indX - planarVoxelHalfWidth) + vehicleX;
            point.y =
                planarVoxelSize * (indY - planarVoxelHalfWidth) + vehicleY;
            point.z = vehicleZ;
            point.intensity = vehicleHeight;

            point.x -= planarVoxelSize / 4.0;
            point.y -= planarVoxelSize / 4.0;
            terrainCloudElev->push_back(point);

            point.x += planarVoxelSize / 2.0;
            terrainCloudElev->push_back(point);

            point.y += planarVoxelSize / 2.0;
            terrainCloudElev->push_back(point);

            point.x -= planarVoxelSize / 2.0;
            terrainCloudElev->push_back(point);
          }
        }
      }

      clearingCloud = false;

      // publish points with elevation
      sensor_msgs::msg::PointCloud2 terrainCloud2;
      pcl::toROSMsg(*terrainCloudElev, terrainCloud2);
      terrainCloud2.header.stamp =
          rclcpp::Time(static_cast<uint64_t>(laserCloudTime * 1e9));
      terrainCloud2.header.frame_id = "odom";
      pubLaserCloud->publish(terrainCloud2);

      // ========== 3D空间障碍物点云生成和发布 ==========
      if (enableSpatialAnalysis) {
        spatialObstacleCloud->clear();
        
        // 遍历3D空间体素，提取障碍物
        for (int indZ = 0; indZ < spatialVoxelHeight; indZ++) {
          for (int indX = 0; indX < spatialVoxelWidth; indX++) {
            for (int indY = 0; indY < spatialVoxelWidth; indY++) {
              int voxelIdx = spatialVoxelWidth * spatialVoxelWidth * indZ +
                           spatialVoxelWidth * indX + indY;
              
              // 只发布占用概率高于阈值的体素
              if (spatialVoxelOccupancy[voxelIdx] > obstacleThreshold) {
                // 计算体素中心坐标
                float voxelCenterX = spatialVoxelSize * (indX - spatialVoxelHalfWidth) + vehicleX;
                float voxelCenterY = spatialVoxelSize * (indY - spatialVoxelHalfWidth) + vehicleY;
                float voxelCenterZ = spatialVoxelSize * (indZ - spatialVoxelHalfHeight) + vehicleZ;
                
                // 创建障碍物点
                pcl::PointXYZI obstaclePoint;
                obstaclePoint.x = voxelCenterX;
                obstaclePoint.y = voxelCenterY;
                obstaclePoint.z = voxelCenterZ;
                obstaclePoint.intensity = spatialVoxelOccupancy[voxelIdx];  // 用intensity表示占用概率
                
                spatialObstacleCloud->push_back(obstaclePoint);
                
                // 为了更好的可视化，在体素的8个角添加点
                float halfSize = spatialVoxelSize / 2.0;
                for (int dx = -1; dx <= 1; dx += 2) {
                  for (int dy = -1; dy <= 1; dy += 2) {
                    for (int dz = -1; dz <= 1; dz += 2) {
                      pcl::PointXYZI cornerPoint;
                      cornerPoint.x = voxelCenterX + dx * halfSize;
                      cornerPoint.y = voxelCenterY + dy * halfSize;
                      cornerPoint.z = voxelCenterZ + dz * halfSize;
                      cornerPoint.intensity = spatialVoxelOccupancy[voxelIdx] * 0.8;  // 稍微降低corner的强度
                      spatialObstacleCloud->push_back(cornerPoint);
                    }
                  }
                }
              }
            }
          }
        }
        
        // 发布3D障碍物点云
        sensor_msgs::msg::PointCloud2 spatialObstacleCloud2;
        pcl::toROSMsg(*spatialObstacleCloud, spatialObstacleCloud2);
        spatialObstacleCloud2.header.stamp =
            rclcpp::Time(static_cast<uint64_t>(laserCloudTime * 1e9));
        spatialObstacleCloud2.header.frame_id = "odom";
        pubSpatialObstacle->publish(spatialObstacleCloud2);
      }
    }

    // status = ros::ok();
    status = rclcpp::ok();
    rate.sleep();
  }

  return 0;
}
